/*
 * @Author: shanghongyun 2879455630@qq.com
 * @Date: 2025-06-12 15:20:19
 * @LastEditors: shanghongyun 2879455630@qq.com
 * @LastEditTime: 2025-06-12 15:22:13
 * @FilePath: \A05\Driver\kalman.c
 * @Description: 卡尔曼滤波
 */
#include "kalman.h"



//2.定义卡尔曼结构体参数并初始化
KFPTypeS kfp_speed = 
{
    20, //估算协方差. 初始化值为 0.02
    0, //卡尔曼增益. 初始化值为 0
    1, //过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏. 初始化值为 0.001
    10000, //测量噪声协方差,R增大，动态响应变慢，收敛稳定性变好. 初始化值为 1
    100 //卡尔曼滤波器输出. 初始化值为 0
};


/**
  ******************************************************************************
  * @brief  卡尔曼滤波器 函数
  * @param  *kfp    - 卡尔曼结构体参数
  * @param  input   - 需要滤波的参数的测量值（即传感器的采集值）
  * @return 卡尔曼滤波器输出值（最优值）
  * @note   
  ******************************************************************************
  */
float KalmanFilter(KFPTypeS *kfp, float input)
{
    //估算协方差方程：当前 估算协方差 = 上次更新 协方差 + 过程噪声协方差
    kfp->P = kfp->P + kfp->Q;
 
    //卡尔曼增益方程：当前 卡尔曼增益 = 当前 估算协方差 / （当前 估算协方差 + 测量噪声协方差）
    kfp->G = kfp->P / (kfp->P + kfp->R);
 
    //更新最优值方程：当前 最优值 = 当前 估算值 + 卡尔曼增益 * （当前 测量值 - 当前 估算值）
    kfp->Output = kfp->Output + kfp->G * (input - kfp->Output); //当前 估算值 = 上次 最优值
 
    //更新 协方差 = （1 - 卡尔曼增益） * 当前 估算协方差。
    kfp->P = (1 - kfp->G) * kfp->P;
 
    return kfp->Output;
}
